
#include "gsl_ext_function.h"
#include "StoppingCriterion.h"

#include <gsl/gsl_blas.h>
#include <stdexcept>

StoppingCriterion::StoppingCriterion(double eps):EPS_(eps) { }

void StoppingCriterion::checkCompatibility(const GSLMinimizerWrapper &s) const
{
  if(!isCompatibleWith(s))
    throw invalid_argument("minimizer is incompatible with stopping criterion");
}

FDistToMinTest::FDistToMinTest(double fMin, double eps, bool relative):
  StoppingCriterion(eps), F_MIN_(fMin), RELATIVE_(relative) { }

double FDistToMinTest::getThresholdValue(const GSLMinimizerWrapper &s) const
{
  StoppingCriterion::checkCompatibility(s);
  
  if(RELATIVE_ == false)
    return s.f() - F_MIN_;
  else
  {
    double v = (s.f() - F_MIN_) / fabs(F_MIN_);
    return v > 0.0 ? v : NAN;
  }
}

bool FDistToMinTest::isCompatibleWith(const GSLFunctionWrapper &f) const { return true; }

bool FDistToMinTest::isCompatibleWith(const GSLMinimizerWrapper &s) const { return true; }

int FDistToMinTest::test(const GSLMinimizerWrapper &s) const
{
  return (getThresholdValue(s) < EPS_) ? GSL_SUCCESS : GSL_CONTINUE;
}

GradientNormTest::GradientNormTest(double eps):StoppingCriterion(eps) { }

double GradientNormTest::getThresholdValue(const GSLMinimizerWrapper &s) const
{
  StoppingCriterion::checkCompatibility(s);
  return gsl_blas_dnrm2(s.gradient());
}

bool GradientNormTest::isCompatibleWith(const GSLFunctionWrapper &f) const { return true; }

bool GradientNormTest::isCompatibleWith(const GSLMinimizerWrapper &s) const
{
  // Cannot use the gradient test with a solver 
  // that does not use function derivatives.
  if(s.getType() == GSLMinimizerWrapper::F)
    return false;
  else
    return true;
}

int GradientNormTest::test(const GSLMinimizerWrapper &s) const
{
  return gsl_multimin_test_gradient(s.gradient(), EPS_);
}

double MaxNumIterTest::getThresholdValue(const GSLMinimizerWrapper &s) const
{
  StoppingCriterion::checkCompatibility(s);
  return s.getNumIter();
}

MaxNumIterTest::MaxNumIterTest(int n):StoppingCriterion(0), MAX_NUM_ITER_(n) { }

bool MaxNumIterTest::isCompatibleWith(const GSLFunctionWrapper &f) const { return true; }

bool MaxNumIterTest::isCompatibleWith(const GSLMinimizerWrapper &s) const { return true; }

int MaxNumIterTest::test(const GSLMinimizerWrapper &s) const
{
  return (s.getNumIter() >= MAX_NUM_ITER_) ? GSL_SUCCESS : GSL_CONTINUE;
}

SimplexSizeTest::SimplexSizeTest(double eps):StoppingCriterion(eps) { }

double SimplexSizeTest::getThresholdValue(const GSLMinimizerWrapper &s) const
{
  StoppingCriterion::checkCompatibility(s);
  return dynamic_cast< const GSLMinimizerWrapper_f & >(s).size();
}

bool SimplexSizeTest::isCompatibleWith(const GSLFunctionWrapper &f) const { return true; }

bool SimplexSizeTest::isCompatibleWith(const GSLMinimizerWrapper &s) const
{
  // Cannot test simplex size on a non-simplex solver.
  if(s.getType() != GSLMinimizerWrapper::F)
    return false;
  else
    return true;
}

int SimplexSizeTest::test(const GSLMinimizerWrapper &s) const
{
  return gsl_multimin_test_size(dynamic_cast< const GSLMinimizerWrapper_f & >(s).size(), EPS_);
}

XDiffTest::XDiffTest(double eps):StoppingCriterion(eps) { }

double XDiffTest::getThresholdValue(const GSLMinimizerWrapper &s) const
{
  StoppingCriterion::checkCompatibility(s);
  
  gsl_vector *dx = NULL;
  
  if(s.getType() == GSLMinimizerWrapper::FDF)
    dx = dynamic_cast< const GSLMinimizerWrapper_fdf & >(s).dx();
  else if(s.getType() == GSLMinimizerWrapper::FD2F)
    dx = dynamic_cast< const GSLMinimizerWrapper_fd2f & >(s).dx();
  
  return gsl_blas_dnrm2(dx);
}

bool XDiffTest::isCompatibleWith(const GSLFunctionWrapper &f) const { return true; }

bool XDiffTest::isCompatibleWith(const GSLMinimizerWrapper &s) const
{
  // Cannot test the difference norm of iteration steps 
  // with simplex algorithms, because they don't store 
  // that information.
  if(s.getType() == GSLMinimizerWrapper::F)
    return false;
  else
    return true;
}

int XDiffTest::test(const GSLMinimizerWrapper &s) const
{
  return (getThresholdValue(s) < EPS_) ? GSL_SUCCESS : GSL_CONTINUE;
}

XDistToMinTest::XDistToMinTest(gsl_vector *xMin, double eps, bool relative):
  StoppingCriterion(eps), RELATIVE_(relative), X_MIN_(xMin)
{
  X_MIN_ = gsl_vector_alloc(xMin->size);
  gsl_blas_dcopy(xMin, X_MIN_);
  tmpv_ = gsl_vector_alloc(xMin->size);
}

XDistToMinTest::~XDistToMinTest()
{
  gsl_vector_free(X_MIN_);
  gsl_vector_free(tmpv_);
}

bool XDistToMinTest::isCompatibleWith(const GSLFunctionWrapper &f) const
{
  return (f.n() == X_MIN_->size);
}

bool XDistToMinTest::isCompatibleWith(const GSLMinimizerWrapper &s) const { return true; }

double XDistToMinTest::getThresholdValue(const GSLMinimizerWrapper &s) const
{
  StoppingCriterion::checkCompatibility(s);
  
  gsl_blas_dcopy(s.x(), tmpv_);
  gsl_blas_daxpy(-1.0, X_MIN_, tmpv_);
  
  if(RELATIVE_ == false)
    return gsl_blas_dnrm2(tmpv_);
  else
    return gsl_blas_dnrm2(tmpv_) / gsl_blas_dnrm2(X_MIN_);
}

int XDistToMinTest::test(const GSLMinimizerWrapper &s) const
{
  return (getThresholdValue(s) < EPS_) ? GSL_SUCCESS : GSL_CONTINUE;
}
